% Dynamic_Params
global m_o g m_p1 m_p2 m_p3 m_p4 m_p
% 货物参数
global J0 
global J0_x J0_y J0_z
global a1_0 a2_0 a3_0
% 无人机参数
global J1_p J2_p J3_p J4_p
global Jx_p Jy_p Jz_p
global a1_p a2_p a3_p l_i
global b1_p b2_p b3_p
% 无人机参数
m_p1 = 0.8;
m_p2 = m_p1;
m_p3 = m_p1;
m_p4 = m_p1;
m_p = [m_p1 m_p2 m_p3 m_p4];

l_i = 1;

J1_p = diag([0.082, 0.0845, 0.1377]);
J2_p = diag([0.082, 0.0845, 0.1377]);
J3_p = diag([0.082, 0.0845, 0.1377]);
J4_p = diag([0.082, 0.0845, 0.1377]);

Jx_p = 0.082;
Jy_p = 0.0845;
Jz_p = 0.1377;

a1_p = (Jy_p-Jz_p)/Jx_p;
a2_p = (Jz_p-Jx_p)/Jy_p; 
a3_p = (Jx_p-Jy_p)/Jz_p;
b1_p = l_i/Jx_p;
b2_p = l_i/Jy_p;
b3_p = 1/Jz_p;

% 货物参数
m_o = 2;
g = 9.81; 
J0 = diag([0.085 0.13 0.2050]);
J0_x = 0.085;
J0_y = 0.13;
J0_z = 0.2050;
a1_0 = (J0_y-J0_z)/J0_x;
a2_0 = (J0_z-J0_x)/J0_y; 
a3_0 = (J0_x-J0_y)/J0_z;



% Control Params
global  Kp_phi   Ki_phi   Kd_phi
global  Kp_tta   Ki_tta   Kd_tta 
global  Kp_psi   Ki_psi   Kd_psi
global  Kp_z     Ki_z     Kd_z  
global  Kp_y     Ki_y     Kd_y      
global  Kp_x     Ki_x     Kd_x                                    

Kp_z = 10*m_o;
Ki_z = 3.5*10^-2;
Kd_z = 10*m_o;

Kp_psi = 15;
Ki_psi = 0.01;
Kd_psi = 15;

Kp_phi = 1;
Ki_phi = 0.001;
Kd_phi = 0.2;

Kp_y = 10*pi/180;%50*pi/180;
Ki_y = 0.0004;
Kd_y = 6*pi/180;%20*pi/180;

Kp_tta = Kp_phi;
Ki_tta = Ki_phi;
Kd_tta = Kd_phi;

Kp_x = 3*pi/180;
Ki_x = 0.000015;
Kd_x = 5*pi/180;%5*pi/180;


global Quadrotor_Num
% Simulation_params
dt = 0.01;
Tf = 80;
Quadrotor_Num = 4;